/**
 * File: geomagnetic.h
 * Brief: this is working for deal the message of geomagnetic sensor.
 * Created by: zhangping
 * Created at: 2017.07.10
 * Modified by:
 * Modified at:
 **/

#include <tf/transform_listener.h> // For tf::Quaternion
#include "geomagnetic.h"
#include <stdio.h>
#include <iostream>

bool GeomagToQuaternion(const GeomagneticData &data
    , geometry_msgs::Quaternion &odom_quat)
{
    double yaw = ((data.yaw_h<<8)|data.yaw_l)*3.1416/32768.0+kGeomagneticOffset;
    std::cerr<<"sensor yaw_radius: "<<yaw<<std::endl;
    // yaw to quaternion
    tf::Quaternion tf_quat;
    tf_quat.setRPY(0,0,yaw);
    tf::quaternionTFToMsg(tf_quat, odom_quat);
    
    return true;
}

void ShowGeomag(const GeomagneticData &data)
{
    printf("data yaw> [%x %x]\n",data.yaw_h, data.yaw_l);
    
}

bool IsValidGeomag(const GeomagneticData &data)
{
    unsigned char sum = data.roll_h   + data.roll_l
               +data.pitch_h + data.pitch_l
               +data.yaw_h   + data.yaw_l
               +data.t_h     + data.t_l;

    return data.sum==sum?true:false;
}

